#include "affine_matrix.h"

_TRT_INFER_BEGIN
class AffineMatrixWapperPrivateData
{
public:
   cv::Mat i2d;
   cv::Mat d2i;
   cv::Mat get_move_matrix(float x, float y);
   cv::Mat get_rotate_matrix(float angle);
   cv::Mat get_scale_matrix(float x, float y);
};

AffineMatrixWapper::AffineMatrixWapper()
{
   data_ = new AffineMatrixWapperPrivateData();
   data_->i2d = cv::Mat::eye(3, 3, CV_32F);
   data_->d2i = cv::Mat::eye(3, 3, CV_32F);
}

AffineMatrixWapper::~AffineMatrixWapper()
{
   if (data_)
   {
      delete data_;
   }
   data_ = nullptr;
}

void AffineMatrixWapper::move(float x, float y)
{
   data_->i2d = data_->get_move_matrix(x, y) * data_->i2d;
   data_->d2i = data_->i2d.inv();
}

void AffineMatrixWapper::rotate(float angle)
{
   data_->i2d = data_->get_rotate_matrix(angle) * data_->i2d;
   data_->d2i = data_->i2d.inv();
}

void AffineMatrixWapper::scale(float x, float y)
{
   data_->i2d = data_->get_scale_matrix(x, y) * data_->i2d;
   data_->d2i = data_->i2d.inv();
}

void AffineMatrixWapper::crop(float x, float y, float w, float h)
{
   data_->i2d = data_->get_move_matrix(-x, -y) * data_->i2d;
   data_->d2i = data_->i2d.inv();
}

size_t AffineMatrixWapper::get_matrix_size()
{
   return sizeof(float) * 6;
}

std::vector<float> AffineMatrixWapper::d2i()
{
   std::vector<float> d2i_vec(6);
   d2i_vec[0] = data_->d2i.at<float>(0, 0);
   d2i_vec[1] = data_->d2i.at<float>(0, 1);
   d2i_vec[2] = data_->d2i.at<float>(0, 2);
   d2i_vec[3] = data_->d2i.at<float>(1, 0);
   d2i_vec[4] = data_->d2i.at<float>(1, 1);
   d2i_vec[5] = data_->d2i.at<float>(1, 2);
   return d2i_vec;
}

std::vector<float> AffineMatrixWapper::i2d()
{
   std::vector<float> i2d_vec(6);
   i2d_vec[0] = data_->i2d.at<float>(0, 0);
   i2d_vec[1] = data_->i2d.at<float>(0, 1);
   i2d_vec[2] = data_->i2d.at<float>(0, 2);
   i2d_vec[3] = data_->i2d.at<float>(1, 0);
   i2d_vec[4] = data_->i2d.at<float>(1, 1);
   i2d_vec[5] = data_->i2d.at<float>(1, 2);
   return i2d_vec;
}

cv::Mat AffineMatrixWapperPrivateData::get_move_matrix(float x, float y)
{
   cv::Mat move_matrix = cv::Mat::eye(3, 3, CV_32F);
   move_matrix.at<float>(0, 2) = x;
   move_matrix.at<float>(1, 2) = y;
   return move_matrix;
}

cv::Mat AffineMatrixWapperPrivateData::get_rotate_matrix(float angle)
{
   cv::Mat rotate_matrix = cv::Mat::eye(3, 3, CV_32F);
   rotate_matrix.at<float>(0, 0) = cos(angle/180.0*CV_PI);
   rotate_matrix.at<float>(0, 1) = -sin(angle/180.0*CV_PI);
   rotate_matrix.at<float>(1, 0) = sin(angle/180.0*CV_PI);
   rotate_matrix.at<float>(1, 1) = cos(angle/180.0*CV_PI);
   return rotate_matrix;
}

cv::Mat AffineMatrixWapperPrivateData::get_scale_matrix(float x, float y)
{
   cv::Mat scale_matrix = cv::Mat::eye(3, 3, CV_32F);
   scale_matrix.at<float>(0, 0) = x;
   scale_matrix.at<float>(1, 1) = y;
   return scale_matrix;
}

_TRT_INFER_END
